/*
 * RiconoscimentoPalla.c
 */

#include "RiconoscimentoPalla.h"


RiconoscimentoPalla::RiconoscimentoPalla(){
}


Point RiconoscimentoPalla::riconoscimento_palla(Mat rgbMat,Mat img_palla_filt2){
	Mat pointsfPalla;
	RotatedRect maxRettangoloPalla,box;
	bool pallaRiconosciuta,inizioRiconoscimento;
	Point centroPalla(NULL,NULL);

	vector<vector<Point> > contours_palla,contours_porta,contours_ostacolo,result;

	findContours(img_palla_filt2, contours_palla, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);

	pallaRiconosciuta=false;
	inizioRiconoscimento=true;

	for(size_t i = 0; i < contours_palla.size(); i++)
	{
		size_t count = contours_palla[i].size();

		if( count < 6 )
			continue;

		Mat(contours_palla[i]).convertTo(pointsfPalla, CV_32F);
		box = fitEllipse(pointsfPalla);
		box.angle = -box.angle;

		if( MAX(box.size.width, box.size.height) > MIN(box.size.width, box.size.height)*30 )
			continue;
		if(box.size.height*box.size.width<MIN_AREA_PALLA)
			continue;
		if(inizioRiconoscimento){
			inizioRiconoscimento=false;
			maxRettangoloPalla=box;
			pallaRiconosciuta=true;
		}else if(box.size.height*box.size.width>maxRettangoloPalla.size.height*maxRettangoloPalla.size.width)
			maxRettangoloPalla=box;
	}

	if(pallaRiconosciuta){
		centroPalla=Point(maxRettangoloPalla.center.x,maxRettangoloPalla.center.y);
		circle( rgbMat, maxRettangoloPalla.center, MIN(maxRettangoloPalla.size.width/2,maxRettangoloPalla.size.height/2), Scalar(255,255,255), 3, 8, 0 );
		circle( rgbMat, maxRettangoloPalla.center, 3, Scalar(255,255,255), -1, 8, 0 );
	}

	return centroPalla;
}

unsigned short RiconoscimentoPalla::calcoloProfonditaPalla(Mat rgbMat,Mat depthMat, Point centroPalla){

	unsigned short profonditaPalla;
	char profonditaPallaString[20000];

	profonditaPalla= ((depthMat.at<unsigned short>(centroPalla)));
	sprintf(profonditaPallaString,"%d mm",profonditaPalla);
	putText(rgbMat, profonditaPallaString, centroPalla, 1, 2, Scalar(255,255,255),1,5,false);

	return profonditaPalla;
}



double RiconoscimentoPalla::calcoloAngoloPalla(Mat rgbMat,CvPoint centroPalla){

	int aPalla, bPalla;
	double dPalla, alfaPalla;
	char angoloPallaString[20000];
	CvPoint cPalla;
	CvPoint ptPalla = cvPoint(rgbMat.cols/2,rgbMat.rows);
	cPalla=centroPalla;

	//Applicazione del teorema dei seni
	bPalla = abs((ptPalla.y) - cPalla.y);
	aPalla = abs((cPalla.x) - ptPalla.x);
	dPalla = sqrt((aPalla*aPalla)+(bPalla*bPalla));
	alfaPalla = asin (aPalla/dPalla);

	//Assegnazione del segno all'angolo a seconda del quadrante in cui si trova
	if((cPalla.x) < rgbMat.cols/2)
		alfaPalla = (360 * (-1)*alfaPalla)/6.28;
	else
		alfaPalla = (360 * alfaPalla)/6.28;

	sprintf(angoloPallaString,"%.2f deg",alfaPalla);
	putText(rgbMat, angoloPallaString, Point(centroPalla.x,centroPalla.y+30), 1, 2, Scalar(255,255,255),1,5,false);


	return alfaPalla;
}


RiconoscimentoPalla::~RiconoscimentoPalla(){

}

